#include "board.h"
#include "utils/utils.h"
#include "utils/circle_buffer.h"
#include "bootloader/protocol.h"
#include "bootloader/iap.h"

typedef struct {
	FDCAN_TxHeaderTypeDef Header;
	u8 data[8];
}can_tx_message_t;

typedef struct {
	FDCAN_RxHeaderTypeDef Header;
	u8 data[64];
}can_rx_message_t;

static FDCAN_HandleTypeDef hfdcan1;

bool can_dev_rx_frame(u8 fifo);

static void MX_FDCAN1_Init(void)
{

	/* USER CODE BEGIN FDCAN1_Init 0 */

	/* USER CODE END FDCAN1_Init 0 */

	/* USER CODE BEGIN FDCAN1_Init 1 */

	/* USER CODE END FDCAN1_Init 1 */
	hfdcan1.Instance = FDCAN1;
	hfdcan1.Init.ClockDivider = FDCAN_CLOCK_DIV1;
	hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
	hfdcan1.Init.Mode = FDCAN_MODE_NORMAL;
	hfdcan1.Init.AutoRetransmission = DISABLE;
	hfdcan1.Init.TransmitPause = DISABLE;
	hfdcan1.Init.ProtocolException = DISABLE;

	hfdcan1.Init.NominalPrescaler = 40;
	hfdcan1.Init.NominalSyncJumpWidth = 1;
	hfdcan1.Init.NominalTimeSeg1 = 2;
	hfdcan1.Init.NominalTimeSeg2 = 14;
	hfdcan1.Init.DataPrescaler = 1;
	hfdcan1.Init.DataSyncJumpWidth = 1;
	hfdcan1.Init.DataTimeSeg1 = 1;
	hfdcan1.Init.DataTimeSeg2 = 1;
	hfdcan1.Init.StdFiltersNbr = 0;
	hfdcan1.Init.ExtFiltersNbr = 0;
	hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
#if CAN_BITRATE == CAN_BITRATE_250K
	hfdcan1.Init.NominalPrescaler = 40;
#elif CAN_BITRATE == CAN_BITRATE_500K
	hfdcan1.Init.NominalPrescaler = 20;
#endif
	if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK)
	{
		//Error_Handler();
	}
	/* USER CODE BEGIN FDCAN1_Init 2 */

	/* USER CODE END FDCAN1_Init 2 */
}

void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef* hfdcan)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
  if(hfdcan->Instance==FDCAN1)
  {
  /* USER CODE BEGIN FDCAN1_MspInit 0 */

  /* USER CODE END FDCAN1_MspInit 0 */

  /** Initializes the peripherals clocks
  */
    LL_RCC_SetFDCANClockSource(LL_RCC_FDCAN_CLKSOURCE_PCLK1);

    /* Peripheral clock enable */
    __HAL_RCC_FDCAN_CLK_ENABLE();

    __HAL_RCC_GPIOB_CLK_ENABLE();
    /**FDCAN1 GPIO Configuration
    PB8-BOOT0     ------> FDCAN1_RX
    PB9     ------> FDCAN1_TX
    */
    /*GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF9_FDCAN1;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
	*/
	gpio2_af_init(PIO(PORTB, PIN8), GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW, GPIO_AF9_FDCAN1);
	gpio2_af_init(PIO(PORTB, PIN9), GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW, GPIO_AF9_FDCAN1);

  /* USER CODE BEGIN FDCAN1_MspInit 1 */

  /* USER CODE END FDCAN1_MspInit 1 */

  }
}
/**
* @brief FDCAN MSP De-Initialization
* This function freeze the hardware resources used in this example
* @param hfdcan: FDCAN handle pointer
* @retval None
*/
void HAL_FDCAN_MspDeInit(FDCAN_HandleTypeDef* hfdcan)
{
  if(hfdcan->Instance==FDCAN1)
  {
  /* USER CODE BEGIN FDCAN1_MspDeInit 0 */

  /* USER CODE END FDCAN1_MspDeInit 0 */
    /* Peripheral clock disable */
    __HAL_RCC_FDCAN_CLK_DISABLE();

    /**FDCAN1 GPIO Configuration
    PB8-BOOT0     ------> FDCAN1_RX
    PB9     ------> FDCAN1_TX
    */
    //HAL_GPIO_DeInit(GPIOB, GPIO_PIN_8|GPIO_PIN_9);
	gpio2_deinit(PIO(PORTB, PIN8));
	gpio2_deinit(PIO(PORTB, PIN9));
  /* USER CODE BEGIN FDCAN1_MspDeInit 1 */

  /* USER CODE END FDCAN1_MspDeInit 1 */
  }

}

bool can_dev_get_error(void) {
    can_dev_rx_frame(FDCAN_RX_FIFO0);
	return (__HAL_FDCAN_GET_FLAG(&hfdcan1, (FDCAN_FLAG_ERROR_PASSIVE|FDCAN_FLAG_ERROR_LOGGING_OVERFLOW|FDCAN_FLAG_BUS_OFF)));
}

void can_dev_config(void) {
	MX_FDCAN1_Init();
	HAL_FDCAN_Start(&hfdcan1);
}

void can_dev_deinit(void) {
    HAL_FDCAN_DeInit(&hfdcan1);
}

void can_dev_init(void) {
    HAL_FDCAN_DeInit(&hfdcan1);
	MX_FDCAN1_Init();
	HAL_FDCAN_Start(&hfdcan1);
}

bool can_dev_tx_frame(can_frame_t *frame) {
    if(HAL_FDCAN_GetTxFifoFreeLevel(&hfdcan1) == 0) {
        return false;
    }
    FDCAN_TxHeaderTypeDef Header;
	Header.Identifier	= frame->efid;
	Header.DataLength  = frame->length;
	Header.IdType		= FDCAN_EXTENDED_ID;
	Header.FDFormat	= FDCAN_CLASSIC_CAN;
	Header.TxFrameType = FDCAN_DATA_FRAME;
	Header.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
	Header.MessageMarker      = 0;
	Header.BitRateSwitch      = FDCAN_BRS_OFF;
	Header.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
	if (HAL_OK != HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &Header, frame->data)){
		return false;
	}
    return true;
}


bool can_dev_rx_frame(u8 fifo)
{
	can_frame_t *frame;
	u8 index;

	if (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, fifo) == 0) {
		return true;
	}

    can_rx_message_t message;

	if (HAL_OK != HAL_FDCAN_GetRxMessage(&hfdcan1, fifo, &message.Header, message.data)){
		return false;
	}
	if (message.Header.FDFormat != FDCAN_CLASSIC_CAN || message.Header.RxFrameType != FDCAN_DATA_FRAME ||
		message.Header.IdType != FDCAN_EXTENDED_ID || message.Header.DataLength > FDCAN_DLC_BYTES_8) {
		return false;
	}

	index = byte_queue_alloc(&can_rx_allocator);
	if (index == 0xFF) {
		return false;
	}

	frame = can_rx_frames + index;
	frame->efid = message.Header.Identifier | (message.Header.DataLength - 1) << 29;
    memcpy(frame->data, message.data, message.Header.DataLength);
	if (byte_queue_write_byte(&can_rx_queue, index)) {
		return true;
	}

	byte_queue_free(&can_rx_allocator, index);
	return true;
}
